#include "commManager.h"

/// @brief 构造
commManager::commManager() {
    /*启动标志*/
    this->RUN_SIG = true;
    /*实例化串口*/
    Uart[0] = new UART();
}

/// @brief 析构
commManager::~commManager() {
    this->RUN_SIG = false;
    for (int i = 0; i < USART_MAX_NUM; i++)
    {
        delete Uart[i];
    }
}


/// @brief 启动:发送,接收,解包三个进程, 需要手动调用
/// @param  
void commManager::begin_sr_thread(void)
{
    /*创建串口发送和接收进程*/
    for (int i = 0; i < USART_MAX_NUM; i++)
    {
        std::thread s(&commManager::send_thread, this, i);
        std::thread r(&commManager::recv_thread, this, i);
        std::thread p(&commManager::recvPacket_thread, this, i);
        s.detach();
        r.detach();
        p.detach();
    }
}

/// @brief CRC校验函数
/// @param data 数据
/// @param len 长度
/// @return 
uint16_t commManager::crc16Calculate(const uint8_t* data, int len) {
    uint16_t crc = 0xFFFF;
    for (int i = 0; i < len; i++) {
        crc ^= (uint16_t)data[i];
        for (int j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc = (crc >> 1) ^ 0xA001;
            }
            else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

/// @brief 将数据打包并发送
/// @param dev_id 驱动设备号
/// @param cmd 指令
/// @param data 数据
/// @param dataLen data长度
/// @return 
int commManager::sendPacket(int dev_id, int cmd, uint8_t* data, int dataLen)
{
    // 计算数据包总长度 = 前缀 + 设备号 + 指令码 + 数据长度 + 数据 + CRC
    int totalLen = PREFIX_LEN + DEV_ID_LEN + CMD_LEN + DATA_LEN_FIELD_LEN + dataLen + CRC_LEN;
    uint8_t* packet = (uint8_t*)malloc(totalLen);
    if (packet == NULL) {
        return -1; // 内存分配失败
    }

    int offset = 0;
    // 1. 填充前缀
    packet[offset++] = PACKET_PREFIX;
    // 2. 填充设备号（新增逻辑）
    packet[offset++] = (uint8_t)dev_id;
    // 3. 填充指令码
    packet[offset++] = (uint8_t)cmd;
    // 4. 填充数据长度（大端模式）
    packet[offset++] = (uint8_t)(dataLen >> 8);
    packet[offset++] = (uint8_t)(dataLen & 0xFF);
    // 5. 填充数据
    memcpy(packet + offset, data, dataLen);
    offset += dataLen;
    // 6. 计算并填充 CRC（注意：CRC 计算范围包含【前缀 + 设备号 + 指令码 + 数据长度 + 数据】）
    uint16_t crc = crc16Calculate(packet, offset);
    packet[offset++] = (uint8_t)(crc >> 8);
    packet[offset++] = (uint8_t)(crc & 0xFF);

    // 发送逻辑 ...
    this->push(dev_id, packet, totalLen);
    free(packet);
    return 0;
}

// 接收状态枚举
typedef enum {
    RECV_STATE_IDLE = 0,        // 空闲状态，等待前缀
    RECV_STATE_DEV,             // 已收到前缀，等待设备号
    RECV_STATE_CMD,             // 已收到设备号, 等待指令码
    RECV_STATE_DATALEN,         // 已收到指令码, 等待告知数据长度
    RECV_STATE_DATA,            // 已收到数据，等待数据
    RECV_STATE_CRC,             // 等待CRC校验码 
    RECV_STATE_DEAL             // 正在处理接收到的数据
} RecvState;

/// @brief 解析收到的数据包并调用对应的函数处理
/// @param dev_id 串口驱动号
/// @return 
int commManager::recvPacket_thread(int dev_id)
{
    int i = 0;
    doCmd handler = NULL;
    uint8_t data = 0;
    uint8_t dev = 0; // 当次收到指令的设备号
    uint8_t cmd = 0; // 当次收到的指令码
    uint16_t data_len = 0; // 当次解析指令数据段的长度
    std::vector<uint8_t> container; // 当次收到的数据临时容器
    uint16_t CRC = 0; // 当次收到的CRC校验码
    std::queue<uint8_t> recv_data;
    RecvState state = RECV_STATE_IDLE;
    
    while (this->RUN_SIG)
    {
        /*从队列中提取信息*/
        data = 0;
        while(this->get(dev_id, data) == 1) 
        {
            recv_data.push(data);
        }

        /*队列为空, 跳过*/
        if (recv_data.empty())continue; 

        /*解析信息*/
        data = recv_data.front();
        recv_data.pop();
        

        /*前缀(1) + 设备号(1) + 指令码(1) + 数据长度(2) + 数据 + CRC(2)*/
        switch (state)
        {   
            case RECV_STATE_IDLE: /*空闲状态，寻找前缀*/
                if (data == PACKET_PREFIX) state = RECV_STATE_DEV;
                i = 0;
                break;
            case RECV_STATE_DEV: /*已收到前缀，等待设备号*/
                dev = data;
                state = RECV_STATE_CMD;
                break;
            case RECV_STATE_CMD: /*已收到设备号, 等待指令码*/
                cmd = data;
                state = RECV_STATE_DATALEN;
                break;
            case RECV_STATE_DATALEN: /*已收到指令码, 等待告知数据长度*/
                data_len |= (uint16_t)data << (8 * (1-i));
                i++;
                if (i == 2) {
                    state = RECV_STATE_DATA;
                    i = 0;
                    container.clear(); // 清空容器数据
                    if (data_len == 0) { // 数据为零的情况下
                        state = RECV_STATE_CRC;
                        container.resize(0);
                    }
                    else 
                    {
                        container.resize(data_len, 0); // 调整容器长度, 并用零填充
                    }
                    
                }
                break;
            case RECV_STATE_DATA: /*等待接收数据*/
                container[i] = data;
                i++;
                if (i == data_len) {
                    state = RECV_STATE_CRC;
                    i = 0;
                }
                break;
            case RECV_STATE_CRC: /*等待CRC校验码*/
                CRC |= (uint16_t)data << (8 * (1 - i));
                i++;
                if (i == 2) {
                    /*CRC校验(待补充)*/
                    state = RECV_STATE_DEAL;
                    i = 0;
                    goto DEAL_CMD;
                }
                break;
            case RECV_STATE_DEAL: /*等待对应函数处理数据*/
                DEAL_CMD:
                /*查找指令对应函数*/
                handler = this->getCmd(cmd);
                if (handler != NULL) {

                    handler(container);
                }

                state = RECV_STATE_IDLE;
                break;
            default:break;
        }
        
    }
    return 0;
}

/// @brief 从接收队列中取得1个字节的数据
/// @param dev_id 串口设备号
/// @param data 用于存放数据的变量
/// @return 1:成功 0:未读取到数据
int commManager::get(int dev_id, uint8_t& data) 
{
    std::lock_guard<std::mutex> lock(this->mutex_r[dev_id]);
    std::queue<char>* rq = &this->rq[dev_id];
    if (!rq->empty())
    {
        data = (uint8_t)rq->front();
        rq->pop();
        return 1;
    }
    return 0;
}

/// @brief 往发送队列中添加数据
/// @param dev_id 对应的串口设备号
/// @param buff 
/// @param len 
void commManager::push(int dev_id, void* buff, int len)
{
    std::lock_guard<std::mutex> lock(this->mutex_s[dev_id]);
    this->sq[dev_id].push(std::string((char*)buff, len));
}

/// @brief 发送数据线程
/// @param dev_id 串口设备号
void commManager::send_thread(int dev_id)
{
    UART* uart = this->Uart[dev_id];
    std::queue<std::string>* sq = &this->sq[dev_id];
    std::string buf;
    uint8_t empty_str = 0x00; // 发送一个空字符
    while (this->RUN_SIG) {
        if (!sq->empty()) {
            std::lock_guard<std::mutex> lock(this->mutex_s[dev_id]);
            buf = sq->front();
            uart->send((void*)buf.c_str(), buf.size());
            uart->send((void*)&empty_str, 1); // 末尾发送一个空白字符
            sq->pop();

            std::this_thread::sleep_for(std::chrono::milliseconds(30)); // 发送间隔 ms
        }
    }
}

/// @brief 接收数据线程
/// @param dev_id 串口设备号
void commManager::recv_thread(int dev_id)
{
    UART* uart = this->Uart[dev_id];
    std::queue<char>* rq = &this->rq[dev_id];
    char buff[1024] = { 0 };
    int num;
    while (this->RUN_SIG) {
        num = uart->recv((void*)buff, 1024);
        if (num > 0)
        {
            std::lock_guard<std::mutex> lock(this->mutex_r[dev_id]);
            for (int i = 0; i < num; i++)
            {
                rq->push(buff[i]);
                buff[i] = 0;
            }
        }
    };
}

/// @brief 插入指令
/// @param cmd 指令码
/// @param handler 该指令码对应的回调函数
void commManager::insertCmd(int cmd, doCmd handler)
{
    for (int i = 0; i < this->cmdTable.size(); i++)
    {
        if (cmdTable[i].cmd == cmd) {
            cmdTable[i].handler = handler; // 替换
            return ;
        }
    }
    CmdEntry t = {cmd, handler};
    this->cmdTable.push_back(t);
}

/// @brief 查找指令码对应的回调函数
/// @param cmd 指令码
/// @return NULL 或者 对应的回调函数
doCmd commManager::getCmd(int cmd)
{
    for (int i = 0; i < this->cmdTable.size(); i++)
    {
        if (cmdTable[i].cmd == cmd) {
            return cmdTable[i].handler;
        }
    }
    return NULL;
}